#include "offboard.hpp"

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_point_z = msg.relative;
}

void imu_callback(const sensor_msgs::Imu &msg)
{
    imu_data = msg;
}



void self_cb(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_point.x= current_msg->pose.pose.position.x;
    current_point.y= current_msg->pose.pose.position.y;
    current_point.z= current_msg->pose.pose.position.z;
    // current_point.z= current_point_z;
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle.x= current_msg->pose.pose.orientation.x;
    current_angle.y= current_msg->pose.pose.orientation.y;
    current_angle.z= current_msg->pose.pose.orientation.z;
    current_angle.w= current_msg->pose.pose.orientation.w;
    current_angle_Q = Eigen::Quaterniond(current_msg->pose.pose.orientation.w, current_msg->pose.pose.orientation.x,
                                        current_msg->pose.pose.orientation.y, current_msg->pose.pose.orientation.z);

    //calculate target test
    geometry_msgs::Point baji_point;
    baji_point.x = 30.0;
    baji_point.y = 30.0;
    baji_point.z = 6.0;
    Eigen::Vector3d p_temp;
    p_temp(0) = baji_point.x-current_point.x;
    p_temp(1) = baji_point.y-current_point.y;
    double horizonal_dist = sqrt(p_temp(0)*p_temp(0)+p_temp(1)*p_temp(1))-5.0;

    if(horizonal_dist<0)
    {
        horizonal_dist = 0.0;
    }
    double hover_height = 4.0;
    if(horizonal_dist>hover_height*3.0)
    {
        horizonal_dist = hover_height*3.0;
    }
    
    p_temp(2) = 10-current_point.z;//(baji_point.z+horizonal_dist/3.0)-current_point.z;
    Eigen::Matrix3d R_yaw;
    R_yaw<<cos(-curr_plane_angle.z),-sin(-curr_plane_angle.z),0,
          sin(-curr_plane_angle.z),cos(-curr_plane_angle.z),0,
          0,0,1;
    Eigen::Quaterniond rotation(R_yaw);
    p_temp = rotation*p_temp;
    mutual_odom = p_temp;
}

void target_cb(const swarm_msgs::BoundingBoxes &msg)
{
    
    float dt_vision = ros::Time::now().toSec()-last_vision_time;
    if(dt_vision>1.0) //dt too long maybe need init dt
    {
        last_vision_time = ros::Time::now().toSec();
        return;
    }
    last_vision_time = ros::Time::now().toSec();
    //计算框大小
    if(msg.bounding_boxes.size()>0)
    {
        double box_time = ros::Time::now().toSec();
        for(int i=0;i<msg.bounding_boxes.size();i++)
        {
            double box_h = msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin;
            double box_v = msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin;
            double box_size = sqrt(box_h * box_h + box_v * box_v);
            // if(box_size<20) //筛选太小的框
            // {
            //     continue;
            // }

            double Bias_H = image_width/2 - (msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0; 
            double Bias_V = image_height/2 - (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0;
    
            double distance = REALSIZE * sqrt(Bias_H*Bias_H + Bias_V*Bias_V + FOCUS*FOCUS) /  box_size;
            std::cout<<"--------------------------------distance->"<<distance<<std::endl;
            double alpha = atan2(Bias_V,FOCUS);
            double beta = atan2(Bias_H,FOCUS);

            //std::cout<<"========>>self angle: "<<beta*180/3.14<<","<<alpha*180/3.14<<std::endl;
            mutual_odom(0) = distance * cos(alpha) * cos(beta);
            mutual_odom(1) = distance * cos(alpha) * sin(beta);
            mutual_odom(2) = distance * sin(alpha);

            std::cout<<"---->camera body point: "<<mutual_odom(0)<<","<<mutual_odom(1)<<","<<mutual_odom(2)<<std::endl;
            Eigen::Matrix3d rotation_vehicle = current_angle_Q.toRotationMatrix();                   
                    
            mutual_odom = rotation_vehicle * mutual_odom;
            
                     
            // Box vision_box;
            // vision_box.x_min = P_Cam(0)-REALSIZE/2.0;
            // vision_box.y_min = P_Cam(1)-REALSIZE/2.0;
            // vision_box.z_min = P_Cam(2)-REALSIZE/2.0;
            // vision_box.x_max = P_Cam(0)+REALSIZE/2.0;
            // vision_box.y_max = P_Cam(1)+REALSIZE/2.0;
            // vision_box.z_max = P_Cam(2)+REALSIZE/2.0;
            // vision_box.id = 0;
            // draw_box(vision_box, i, 0.2, 0, 0.5, 0.5);
            std::cout<<"---->vehicle local point: "<<mutual_odom(0)<<","<<mutual_odom(1)<<","<<mutual_odom(2)<<std::endl;

            // desire where to track
            double tar_angle = atan2(mutual_odom(1),mutual_odom(0));
            mutual_odom(0) = mutual_odom(0)+10*cos(tar_angle);
            mutual_odom(1) = mutual_odom(1)+10*sin(tar_angle);
            mutual_odom(2) = mutual_odom(2);

        }
        have_target_time = ros::Time::now().toSec();

    }
}

void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel1_value = rcvalue.channels[0]; //roll
    channel2_value = rcvalue.channels[1]; //pitch
    channel3_value = rcvalue.channels[2]; //thrust
    channel4_value = rcvalue.channels[3]; //yaw
    channel5_value = rcvalue.channels[4];
    channel6_value = rcvalue.channels[5];
    channel7_value = rcvalue.channels[6];
    channel8_value = rcvalue.channels[7];
}

void px4_control_callback(const ros::TimerEvent& /*event*/) 
{
    // printFSMExecState();
    if(print_state_cnt>100)
    {
        //print state per second
        printFSMExecState();
        print_state_cnt = 0;
    }
    print_state_cnt++;
    
    switch (exec_state) { 
    case INIT: {
        if(use_sitl)
        {
            changeFSMExecState(TAKEOFF, "offboard_fsm");
        }
        else
        {
            changeFSMExecState(CRUISE, "offboard_fsm");
        }
    break;
    }
    case TAKEOFF: {
        if( current_state.mode != "OFFBOARD" &&
        (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_offboard_request = ros::Time::now();
        } 
        else {
            if( !current_state.armed &&
                (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed.");
                }
                last_offboard_request = ros::Time::now();
            }
        }
        geometry_msgs::Point takeoff_position;
        takeoff_position.x = 0;
        takeoff_position.y = 0;
        takeoff_position.z = (target_height-current_point.z);
        att_pid_control(takeoff_position,0,true);
        att_command_send(thrust_expected,  attitude_expect);

        if(current_point.z>9.0)
        {
            changeFSMExecState(ATTACK, "offboard_fsm");
        }
       
    break;
    }

    case CRUISE: {
        set_cruise_state();
        if(current_state.mode == "OFFBOARD")
        {
            changeFSMExecState(ATTACK, "offboard_fsm");  
        }
    break;
    }

    case ATTACK: {
        geometry_msgs::Point error_position;
        error_position.x = mutual_odom(0);
        error_position.y = mutual_odom(1);
        error_position.z = mutual_odom(2);
        att_pid_control(error_position,0.78,false);
        att_command_send(thrust_expected,  attitude_expect);
        
    break;
    }

    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "off_board_test1");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    //read parameters from launch file
    nh.param<bool>("use_sitl",use_sitl,false);

    nh.param<double>("P_pos",P_pos,1.0);
    nh.param<double>("I_pos",I_pos,0.0);
    nh.param<double>("D_pos",D_pos,0.0);
    nh.param<double>("I_zone",I_zone,1.0);

    nh.param<int>("image_width",image_width,1920);
    nh.param<int>("image_height",image_height,1080);
    
    //Subscriber
    altitude_sub = nh.subscribe("/mavros/altitude",10,altitude_callback);
    rcin_sub = nh.subscribe("/mavros/rc/in",10,rcin_callback);
    state_sub = nh.subscribe("/mavros/state", 10, state_cb);
    imu_sub = nh.subscribe("/mavros/imu/data",10,imu_callback);
    target_sub = nh.subscribe("/intercept/target", 10, target_cb);
    self_sub = nh.subscribe("/mavros/local_position/odom", 10, self_cb);


    //Publisher
    raw_pub_att = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);
    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 1);
    control_plot = nh.advertise<std_msgs::Float32MultiArray>("/plot/controller",10);

    goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/zhou/goal1", 10);

    //Service
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("/mavros/setpoint_velocity/mav_frame");

    //Timer
    px4_control_timer = nh.createTimer(ros::Duration(0.05), px4_control_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"------------>wait for fcu connected..."<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }
    
    offb_set_mode.request.custom_mode = "OFFBOARD";
    arm_cmd.request.value = true;
    setmavframe.request.mav_frame = 8;
    last_offboard_request = ros::Time::now();
    //set mavframe to body_ned 8
    // if(set_vechile_ned.call(setmavframe) && setmavframe.response.success)
    // {
    //     std::cout<<"------------>setmavframe success..."<<std::endl;
    // }
    // else{
    //     std::cout<<"------------>setmavframe failed"<<std::endl;
    // }

    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}




void changeFSMExecState(FSM_STATE new_state, std::string pos_call) {

  int    pre_s        = int(exec_state);
  exec_state         = new_state;
  std::cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << std::endl;
}


void printFSMExecState() {
  std::cout << "state: " + state_str[int(exec_state)] << std::endl;
}


void att_command_send(double thrust,  geometry_msgs::Point enu_euler)
{
    mavros_msgs::AttitudeTarget raw_att;
    geometry_msgs::Quaternion q;
    q = euler_to_quat(enu_euler);

    raw_att.header.stamp = ros::Time::now();

    raw_att.thrust=thrust;
    raw_att.orientation.x=q.x;
    raw_att.orientation.y=q.y;
    raw_att.orientation.z=q.z;
    raw_att.orientation.w=q.w;
    raw_att.type_mask = 7;
    raw_pub_att.publish(raw_att);
}

void set_cruise_state()
{
    geometry_msgs::Point static_state;
    static_state.x = 0.0;
    static_state.y = 0.0;
    static_state.z = 0.0;
    att_pid_control(static_state,0,true);
    att_command_send(thrust_expected,  attitude_expect);
  
}


void att_pid_control(geometry_msgs::Point error_position, double yaw_angle, bool sepecified_yaw)
{
    bool heading_target = false;
    dt_calc = ros::Time::now().toSec()-time_last_calc;
    
    if(dt_calc>1.0) //dt too long maybe need init dt
    {
        time_last_calc = ros::Time::now().toSec();
        return;
    }
    double vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim=0.0, vy_lim=0.0;
    double yaw_rate = 0.0, yaw_rate_p = 0.0;
    double vz = 0.0,vz_p=0.0,vz_i=0.0,vz_d=0.0;
    geometry_msgs::Point error_position_limit;
    //limit error position xy
    error_position_limit.x = limit((double)error_position.x, (double)5.0);
    error_position_limit.y = limit((double)error_position.y, (double)5.0);
    //calculate velocity, P control
    double p_y = P_pos;
    double d_y = D_pos;
    if(sqrt(error_position.x*error_position.x+error_position.y*error_position.y)>10.0)
    {
        d_y = 0;
        p_y = 0.1*P_pos;
        
    }
    vxp = P_pos * error_position_limit.x;
    vyp = p_y * error_position_limit.y;
    vxd = D_pos*(error_position_limit.x-error_pos_last.x)/dt_calc;
    vyd = d_y*(error_position_limit.y-error_pos_last.y)/dt_calc;
    vx = (vxp  + vxd);
    vy = (vyp  + vyd);
    //limit the speed
    vx_lim = limit((double)vx, (double)20*deg2rad);
    vy_lim = limit((double)vy, (double)20*deg2rad);

    //yaw control
    double error_yaw = atan2(error_position.y,error_position.x); //yaw calculation need no limit value

    //height control 
    double hover_thrust = 0.6;
    error_position_limit.z = limit((double)error_position.z, (double)5.0);
    vz_p = P_z * error_position_limit.z; 
    vz_d = D_z *(error_position_limit.z-error_pos_last.z)/dt_calc;  
    double v_pid = vz_p+vz_d+hover_thrust/cos(curr_plane_angle.x)/cos(curr_plane_angle.y);
    //fit attitude
    // v_pid = v_pid/cos(curr_plane_angle.x)/cos(curr_plane_angle.y);
    double v_pid_limit = std::max(0.0,std::min(1.0,v_pid));

    

    attitude_expect.x = -vy_lim; //roll angle diverse
    attitude_expect.y = vx_lim;
    if(sepecified_yaw)
    {
        attitude_expect.z = yaw_angle;
    }
    else
    {
        if(sqrt(error_position.x*error_position.x+error_position.y*error_position.y)>10.0)
        {
            std::cout<<"---->heading to target!"<<std::endl;
            attitude_expect.z = error_yaw+curr_plane_angle.z;
        }
        else
        {
            std::cout<<"---->keep current yaw!"<<std::endl;
            attitude_expect.z = curr_plane_angle.z;
        }
        
    }
    
    thrust_expected = v_pid_limit;
    // std::cout<<"----->command: "<<attitude_expect.x<<","<<attitude_expect.y<<","<<attitude_expect.z<<","<<thrust_expected<<std::endl;
    last_calc_time = ros::Time::now().toSec();
    //control plot 
    std_msgs::Float32MultiArray plot_msg;
    plot_msg.data.push_back(error_position_limit.x); //error forward
    plot_msg.data.push_back(attitude_expect.y); //pitch
    plot_msg.data.push_back(error_position_limit.y); //error left
    plot_msg.data.push_back(attitude_expect.x); //roll
    plot_msg.data.push_back(error_position_limit.z); //error up
    plot_msg.data.push_back(thrust_expected);  //thrust
    plot_msg.data.push_back(error_yaw); 
    plot_msg.data.push_back(curr_plane_angle.z); 
    plot_msg.data.push_back(attitude_expect.z); 

    control_plot.publish(plot_msg);

    error_pos_last = error_position_limit;
    
    time_last_calc = ros::Time::now().toSec();
}



//eluer to quad
geometry_msgs::Quaternion euler_to_quat(geometry_msgs::Point euler_angle_enu)
{
    tfScalar yaw,pitch,roll;
    tf::Quaternion q;
    geometry_msgs::Quaternion quat;

    roll = euler_angle_enu.x;
    pitch = euler_angle_enu.y;
    yaw = euler_angle_enu.z;

    q.setEulerZYX(yaw,pitch,roll);
    quat.x = q.x();
    quat.y = q.y();
    quat.z = q.z();
    quat.w = q.w();
    return quat;
} 





